close all
clear all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Automatic Calculation of F with RANSAC, Corner Detection and Feature Matching %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Federico Camposeco Paulsen, VIBOT6
%% Load Images, make them grayscale double
image1_rgb  = imread('shed1.png');
image2_rgb = imread('shed2.png');
if isrgb(image1_rgb)
    image1 = double(rgb2gray(image1_rgb));
else
    image1 = double(image1_rgb);
end
if isrgb(image2_rgb)
    image2 = double(rgb2gray(image2_rgb));
else
    image2 = double(image2_rgb);
end
%% Get Corners (simple harris implementation)
thresh = 80; %threshold for harris detection
x_crn = myHarris(image1,thresh);
xp_crn = myHarris(image2,thresh);% my harris is only 1.64 seconds =)
%% Show Corners
subplot(1,2,1)
imshow(image1_rgb);
title('Found Corners Image 1')
hold on; 
plot(x_crn(:,1), x_crn(:,2), '*y'); 
subplot(1,2,2)
imshow(image2_rgb);
title('Found Corners Image 2')
hold on; 
plot(xp_crn(:,1), xp_crn(:,2), '*y');
%% Find Putative Correlations (simple correlation) and Plot
pmatches = getMatches(x_crn, xp_crn, image1, image2, 9, 120, 0.8);%indeces of matches
x = x_crn(pmatches(:,1),:);
xp = xp_crn(pmatches(:,2),:);
figure,
imshow(image1_rgb + .5*image2_rgb);
str = sprintf('Putative Correlations Found: %d',length(pmatches));
title(str)
hold on; 
step = 1;
line([x(1:step:end,1) xp(1:step:end,1)]', [x(1:step:end,2) xp(1:step:end,2)]', 'Color', 'r');
%% Robust Fitting using RANSAC and Plotting
dThresh = 0.85;
[F,inliers] = RANSAC_F(x,xp,dThresh);

subplot(1,2,1), imagesc(image1); title('Image 1, click to see corresponing epipolar line');colormap(gray);axis image;
subplot(1,2,2), imagesc(image2); title('Image 2, epipolar lines');colormap(gray);axis image;

while 1 
    subplot(1,2,1)%left image
    [px, py] = ginput(1);
    hold on; plot(px,py,'ro');
    m = [px py 1];%to homogeneous
    l = F*m'; %points in right image
    subplot(1,2,2)
    plotline(l);
end



